package robot;

import java.awt.Color;
import java.awt.Font;
import java.util.ArrayList;
import java.util.Random;

import renderables.RenderableOval;
import renderables.RenderablePoint;
import renderables.RenderablePolygon;
import renderables.RenderablePolyline;
import renderables.RenderableString;
import easyGui.EasyGui;
import geometry.IntPoint;

public class ToolBoxDemo {
	public final EasyGui gui;
	
	private RenderablePoint goldPoint; 
	
	private Robot robot;
	
	RenderableOval rendOval;
	
	private int goalRadius;
	
	private ArrayList<Obstacle> obstacles = new ArrayList<Obstacle> ();
	
	RenderablePolyline rendPLine;
	
	Font font; 
	
	// tree
	RRT rrt;
	
	int textFieldX;
	int textFieldY;
	
	int numberOfMovements = 0;
	int lengthOfPath = 0;
	int numberOfTurns = 0;
	
	public ToolBoxDemo() {
		// Create a new EasyGui instance with a 500x500pixel graphics panel.
		gui = new EasyGui(800, 800);
		
		// Add a label to the cell in row 0 and column 0.
//		gui.addLabel(0, 0, "X: ");
//		gui.addLabel(1, 0, "Y: ");
		
		// Add a label to the cell in row 0 and column 0.
//		textFieldX = gui.addTextField(0, 1, "50");
//		textFieldY = gui.addTextField(1, 1, "50");

		gui.addButton(0, 2, "Potential Free Space", this, "potentialFreeSpace");
		
		// Add a button in row 1 column 1. It calls the method called "buttonActionB".
		gui.addButton(1, 2, "Potential Obstacle Space", this, "potentialObstacle");
		
		gui.addButton(0, 3, "RRT Free Space", this, "rrtFreeSpace");
		gui.addButton(1, 3, "RRT Obstacle Space", this, "rrtObstacle");
		
		font = new Font ("SansSerif", Font.PLAIN , 9);	
		
		rrt = new RRT(gui);
	}
	
	public void runDemo() {
		// Displays the GUI i.e. makes it visible.
		gui.show();
	}
	
	public void rrtFreeSpace() {
		rrt.freeSpaceTree();
	}
	
	public void rrtObstacle() {
		rrt.obstacleSpace();
	}

	// MAIN
	public static void main(String[] args) {
		ToolBoxDemo demo = new ToolBoxDemo();
		demo.runDemo();
	}
	
	
	// ****************************************************************************************************************
	// FREE SPACE POTENTIAL
	// ****************************************************************************************************************
		
	public RenderablePoint nextStepFreeSpace() {

		directionPoints(robot.getCoordinates());
		calculatingGoldPotentialForFreeSpace();

//		System.out.println("Coordinates: " + robot.rayPoints[getMaxGoldPotential()].getPoint().x + " " + robot.rayPoints[getMaxGoldPotential()].getPoint().y);
		return makeStepInFreeSpace();	
	}
	
	// make a step towards the best potential distance free space
	public RenderablePoint makeStepInFreeSpace () {	
		
		RenderablePoint futurePoint = robot.rayPoints[getMaxGoldPotential()].getPoint();
		RenderablePoint iP =  new RenderablePoint((int) ((futurePoint.x + robot.getCoordinates().x )/2), (int) ((futurePoint.y + robot.getCoordinates().y )/2));
		
		// calculate distance
		lengthOfPath += (int) calculateDistanceBetweenPoints(robot.getCoordinates().x, robot.getCoordinates().y, iP.x, iP.y);
		
		robot.getCoordinates().x = iP.x;
		robot.getCoordinates().y = iP.y;	
		return iP;
	}
	
	// calculating gold potential and put fill goldPotential array
	public void calculatingGoldPotentialForFreeSpace() {
		for(int i = 0; i < robot.rayPoints.length; i++)		
			robot.rayPoints[i].setGoldPotential(
					calculateDistanceBetweenPoints(goldPoint.x, goldPoint.y, 
							robot.rayPoints[i].getPoint().x, robot.rayPoints[i].getPoint().y));
		
	}
	
	
	public void potentialFreeSpace() {
		numberOfMovements = 0;
		lengthOfPath = 0;
		numberOfTurns = 0;
		checkXYParameters();
		
		gui.clearGraphicsPanel();
        obstacles.clear();
		rendPLine = new RenderablePolyline();
		
		robot = new Robot(textFieldX, textFieldY, 0, 50, 200);

		goldPoint = new RenderablePoint(500, 500);
		goalRadius = 50;
		goldPoint.setProperties(Color.RED, 10.0f);
		gui.draw(goldPoint);

		RenderableOval rO = new RenderableOval((int) (goldPoint.x), (int)(goldPoint.y), 50, 50);
		rO.setProperties(Color.BLUE, 10.0f, false);
		gui.draw(rO);

		rendPLine.addPoint(robot.getCoordinates());
		
		RenderableString start = new RenderableString ((int)(robot.getCoordinates().x), (int)(robot.getCoordinates().y), 
				"(" + ((int)robot.getCoordinates().x) + "," + ((int)robot.getCoordinates().y) + ")");
		gui.draw(start);
		
		// free space!
		freeSpacePotential();
		
		System.out.println("Number of movements: " + numberOfMovements);
		System.out.println("Length of path: " + lengthOfPath);
		System.out.println("Length of turns: " + numberOfTurns);
		
		gui.update();
	}
	
	public void freeSpacePotential() {	
		RenderablePoint point;
		while (!checkIfReachGoldCircle()) {
			
			point = nextStepFreeSpace();
			rendPLine.addPoint(point);	
			RenderableString stringForPath = new RenderableString ((int)point.x, (int)point.y, 
					"(" + ((int)point.x) + "," + ((int)point.y) + ")");
			stringForPath.setProperties(Color.BLACK, font);
			
			// increase movements
			numberOfMovements ++;
			
			gui.draw(stringForPath);
		}
		gui.draw(rendPLine);		
	}
	
	// ****************************************************************************************************************
	// OBSTACLE POTENTIAL
	// ****************************************************************************************************************
	
	
	public void potentialObstacle() {
		numberOfMovements = 0;
		lengthOfPath = 0;
		numberOfTurns = 0;
		checkXYParameters();
		
		gui.clearGraphicsPanel();
        obstacles.clear();
		rendPLine = new RenderablePolyline();
		
		robot = new Robot(textFieldX, textFieldY, 0, 50, 200);
		
		RenderableString start = new RenderableString ((int)(robot.getCoordinates().x), (int)(robot.getCoordinates().y), 
				"(" + ((int)robot.getCoordinates().x) + "," + ((int)robot.getCoordinates().y) + ")");
		gui.draw(start);
		
		Obstacle obstacle1 = new Obstacle(80, 200, 300, 200, 300, 120, 80, 120);
		obstacles.add(obstacle1);
		RenderablePolygon obstaclePolygon1 = obstacle1.print();
		gui.draw(obstaclePolygon1);
		
		Obstacle obstacle2 = new Obstacle(400, 400, 460, 400, 460, 440, 400, 440);
		obstacles.add(obstacle2);
		RenderablePolygon obstaclePolygon2 = obstacle2.print();
		gui.draw(obstaclePolygon2);

		
		Obstacle obstacle3 = new Obstacle(400, 300, 460, 300, 460, 330, 400, 330);
		obstacles.add(obstacle3);
		RenderablePolygon obstaclePolygon3 = obstacle3.print();
		gui.draw(obstaclePolygon3);
		
//		obstacle4 = new Obstacle(240, 240, 300, 240, 300, 280, 240, 280);
//		obstacles.add(obstacle4);
//		RenderablePolygon obstaclePolygon4 = obstacle4.print();
//		gui.draw(obstaclePolygon4);
		
		Obstacle obstacle5 = new Obstacle(300, 480, 330, 480, 330, 410, 300, 410);
		obstacles.add(obstacle5);
		RenderablePolygon obstaclePolygon5 = obstacle5.print();
		gui.draw(obstaclePolygon5);
		
		Obstacle obstacle6 = new Obstacle(550, 550, 600, 550, 600, 600, 550, 600);
		obstacles.add(obstacle6);
		RenderablePolygon obstaclePolygon6 = obstacle6.print();
		gui.draw(obstaclePolygon6);
		
		Obstacle obstacle7 = new Obstacle(580, 300, 630, 300, 630, 380, 580, 380);
		obstacles.add(obstacle7);
		RenderablePolygon obstaclePolygon7 = obstacle7.print();
		gui.draw(obstaclePolygon7);
		
		Obstacle obstacle8 = new Obstacle(100, 650, 160, 650, 160, 700, 100, 700);
		obstacles.add(obstacle8);
		RenderablePolygon obstaclePolygon8 = obstacle8.print();
		gui.draw(obstaclePolygon8);
		
		goldPoint = new RenderablePoint(500, 500);
		goalRadius = 50;
		goldPoint.setProperties(Color.RED, 10.0f);
		gui.draw(goldPoint);

		RenderableOval rO = new RenderableOval((int) (goldPoint.x), (int)(goldPoint.y), 50, 50);
		rO.setProperties(Color.BLUE, 10.0f, false);
		gui.draw(rO);

		rendPLine.addPoint(robot.getCoordinates());

	
		
		// for every type button

//		System.out.println(numberPressed);
//		for (numberOfStep = 0; numberOfStep < numberPressed; numberOfStep++) {
//
//			if (numberOfStep > 0)
//				rendPLine.addPoint(makeStepInObstacleSpace());
//				
//			directionPoints(robot.getCoordinates());
//			calculatingGoldPotential();
//
//			for (int i = 0; i < robot.rayPoints.length; i++)
//				checkHits(i, calculateHitsWithObstacle(i, obstacle1));
//
//			calculateObsticalPotential();
//			getMaxPotential();
//			calculatingSumPotential();
//
//		}
		
		// for viewing all way at once
		for (int numberOfStep = 0; numberOfStep < 1000 ; numberOfStep++) {
		
			if (!checkIfReachGoldCircle()) {
				numberOfMovements ++;
			if (numberOfStep > 0) {
				RenderablePoint point = makeStepInObstacleSpace();
				rendPLine.addPoint(point);
				RenderableString stringForPath = new RenderableString ((int)point.x, (int)point.y, 
						"(" + ((int)point.x) + "," + ((int)point.y) + ")");
				stringForPath.setProperties(Color.BLACK, font);
				gui.draw(stringForPath);
			}
				
			directionPoints(robot.getCoordinates());
			calculatingGoldPotential();

			for (int i = 0; i < robot.rayPoints.length; i++) {
				for (Obstacle obs: obstacles) {
				checkHits(i, calculateHitsWithObstacle(i, obs));
				}
			}

			calculateObsticalPotential();
			getMaxPotential();
			calculatingSumPotential();		
			}
		}
//		rendPLine.addPoint(new RenderablePoint(goldPoint.x, goldPoint.y));
		gui.draw(rendPLine);	
		
		System.out.println("Number of movements: " + numberOfMovements);
		System.out.println("Length of path: " + lengthOfPath);
		System.out.println("Length of turns: " + numberOfTurns);
		gui.update();	
	}

	// calculate obstacle potential
	public void calculateObsticalPotential () {
		
		for(int i = 0; i < robot.rayPoints.length; i++) {	
			double obstaclePotential = 0;
			for (int j=0; j<robot.rayPoints[i].hitPoints.size(); j++) {
				RenderablePoint rp = robot.rayPoints[i].hitPoints.get(j);
				double potential = obstPotential(robot.rayPoints[i].getPoint(), rp); 
				obstaclePotential += potential;
			}
			robot.rayPoints[i].setObstaclePotential(obstaclePotential);
		}
	}
	
	public double obstPotential(RenderablePoint rayPoint, RenderablePoint hitPoint) {
		// need to know distance between sensor point and gold
		double d = calculateDistanceBetweenPoints(rayPoint.x, rayPoint.y, hitPoint.x, hitPoint.y);
	    double a = robot.getSensorRadius() - d;
	    
	    // this formula comes from the Robotics Exercises
	    return -1000 * Math.exp(-1.0/a) / d;
	}
	
	// calculating sum of Gold potential and Obstacle potential
	public void calculatingSumPotential() {
		for(int i = 0; i < robot.rayPoints.length; i++) {			
			robot.rayPoints[i].setSumPotentials(robot.rayPoints[i].getGoldPotential() + robot.rayPoints[i].getObstaclePotential());
			double sum = robot.rayPoints[i].getGoldPotential() + robot.rayPoints[i].getObstaclePotential();
			double golPot = robot.rayPoints[i].getGoldPotential();
			double obstPot = robot.rayPoints[i].getObstaclePotential();
			System.out.printf("R%d S:%.2f G:%.2f O:%.2f\n", i, sum, golPot, obstPot);
			}
	}
	
	public static boolean linesIntersect(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4){
		      // Return false if either of the lines have zero length
		      if (x1 == x2 && y1 == y2 ||
		            x3 == x4 && y3 == y4){
		         return false;
		      }
		      // Fastest method, based on Franklin Antonio's "Faster Line Segment Intersection" topic "in Graphics Gems III" book (http://www.graphicsgems.org/)
		      double ax = x2-x1;
		      double ay = y2-y1;
		      double bx = x3-x4;
		      double by = y3-y4;
		      double cx = x1-x3;
		      double cy = y1-y3;

		      double alphaNumerator = by*cx - bx*cy;
		      double commonDenominator = ay*bx - ax*by;
		      if (commonDenominator > 0){
		         if (alphaNumerator < 0 || alphaNumerator > commonDenominator){
		            return false;
		         }
		      }else if (commonDenominator < 0){
		         if (alphaNumerator > 0 || alphaNumerator < commonDenominator){
		            return false;
		         }
		      }
		      double betaNumerator = ax*cy - ay*cx;
		      if (commonDenominator > 0){
		         if (betaNumerator < 0 || betaNumerator > commonDenominator){
		            return false;
		         }
		      }else if (commonDenominator < 0){
		         if (betaNumerator > 0 || betaNumerator < commonDenominator){
		            return false;
		         }
		      }
		      if (commonDenominator == 0){
		         // This code wasn't in Franklin Antonio's method. It was added by Keith Woodward.
		         // The lines are parallel.
		         // Check if they're collinear.
		         double y3LessY1 = y3-y1;
		         double collinearityTestForP3 = x1*(y2-y3) + x2*(y3LessY1) + x3*(y1-y2);   // see http://mathworld.wolfram.com/Collinear.html
		         // If p3 is collinear with p1 and p2 then p4 will also be collinear, since p1-p2 is parallel with p3-p4
		         if (collinearityTestForP3 == 0){
		            // The lines are collinear. Now check if they overlap.
		            if (x1 >= x3 && x1 <= x4 || x1 <= x3 && x1 >= x4 ||
		                  x2 >= x3 && x2 <= x4 || x2 <= x3 && x2 >= x4 ||
		                  x3 >= x1 && x3 <= x2 || x3 <= x1 && x3 >= x2){
		               if (y1 >= y3 && y1 <= y4 || y1 <= y3 && y1 >= y4 ||
		                     y2 >= y3 && y2 <= y4 || y2 <= y3 && y2 >= y4 ||
		                     y3 >= y1 && y3 <= y2 || y3 <= y1 && y3 >= y2){
		                  return true;
		               }
		            }
		         }
		         return false;
		      }
		      return true;
		   }
	   
	private static RenderablePoint segmentIntersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) {
	   if (linesIntersect(x1,y1,x2,y2,x3,y3,x4,y4)) {
		double det1And2 = det(x1, y1, x2, y2);
	      double det3And4 = det(x3, y3, x4, y4);
	      double x1LessX2 = x1 - x2;
	      double y1LessY2 = y1 - y2;
	      double x3LessX4 = x3 - x4;
	      double y3LessY4 = y3 - y4;
	      double det1Less2And3Less4 = det(x1LessX2, y1LessY2, x3LessX4, y3LessY4);
	      if (det1Less2And3Less4 == 0){
	         // the denominator is zero so the lines are parallel and there's either no solution (or multiple solutions if the lines overlap) so return null.
	         return null;
	      }
	      double x = (det(det1And2, x1LessX2,
	            det3And4, x3LessX4) /
	            det1Less2And3Less4);
	      double y = (det(det1And2, y1LessY2,
	            det3And4, y3LessY4) /
	            det1Less2And3Less4);
	      return new RenderablePoint((int)x, (int)y);
	   } else return null;
	   }
	
	   private static double det(double a, double b, double c, double d) {
	      return a * d - b * c;
	   }	
	   
	// do this function for all XRAYS
	public RenderablePoint calculateHitsWithObstacle (int numberOfRay, Obstacle obstacle) {
		
		double x1 = robot.rayPoints[numberOfRay].getPoint().x;
		double y1 = robot.rayPoints[numberOfRay].getPoint().y;
		double x2 = goldPoint.x;
		double y2 = goldPoint.y;
		
		RenderablePoint closest = null;
		double minDistance = Double.MAX_VALUE;
		
		for (int i=0; i<4; i++) {
			double x3 = obstacle.vertex[i].x;
			double y3 = obstacle.vertex[i].y;
			double x4 = obstacle.vertex[(i+1)%4].x;
            double y4 = obstacle.vertex[(i+1)%4].y;
			RenderablePoint p = segmentIntersection(x1,y1,x2,y2,x3,y3,x4,y4);
			if (p!=null) {
				double d = Math.sqrt((x1-p.x)*(x1-p.x)+(y1-p.y)*(y1-p.y));
				if (d<minDistance) {
					closest = p;
					minDistance = d;
				}
			}
		}
		
		return closest;
	}
	
	// check if hits are inside sensor area of robot
	public void checkHits(int rayPoint, RenderablePoint p) {
		if (p!=null && robot.isPointInsideSensorArea(p))
			robot.rayPoints[rayPoint].addHitPoint(p);
	}
	
	public double calculateDistanceBetweenPoints(double x1, double y1, double x2, double y2) {
		return Math.sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
	}
		
	// make a step towards the best potential distance obstacle space
	public RenderablePoint makeStepInObstacleSpace () {	
		RenderablePoint futurePoint = robot.rayPoints[getMaxPotential()].getPoint();
		RenderablePoint iP =  new RenderablePoint((futurePoint.x + robot.getCoordinates().x )/2, (futurePoint.y + robot.getCoordinates().y )/2);
		
		// calculate distance
		lengthOfPath += (int) calculateDistanceBetweenPoints(robot.getCoordinates().x, robot.getCoordinates().y, iP.x, iP.y);
		
		robot.getCoordinates().x = iP.x;
		robot.getCoordinates().y = iP.y;	
		return iP;
	}
	
	// ****************************************************************************************************************
	// METHODS FOR BOTH: OBSTACLE AND FREE SPACE 
	// ****************************************************************************************************************
	
	public int getMaxPotential() {
		double maxPot = Double.NEGATIVE_INFINITY;
		int numberOfElement = -1;
		for(int i = 0; i < robot.rayPoints.length; i++) {
			if (robot.rayPoints[i].getSumPotentials() > maxPot) {
				maxPot = robot.rayPoints[i].getSumPotentials();
				numberOfElement = i;
			}
		}
		
		if (numberOfElement != 3) {
			numberOfTurns ++;
		}
		
		System.out.println("Angle: " + numberOfElement);
		robot.setAngle(robot.rayPoints[numberOfElement].getAngle());
		return numberOfElement;
	}
	
	
	// fill the array with point of X-Ray
	public void directionPoints(RenderablePoint robotPoint) {
		
		double  a = Math.PI/2 + robot.getAngle();
		for(int i = 0; i < robot.rayPoints.length; i++) {		
			double x = robot.getCoordinates().x  +  Math.cos(a) * robot.getRayRadius();
			double y = robot.getCoordinates().y + Math.sin(a) * robot.getRayRadius();		
			robot.rayPoints[i] = new RayPoint();
			robot.rayPoints[i].setPoint(new RenderablePoint((int)x, (int)y));
			robot.rayPoints[i].setAngle(a);
			
//			if (numberOfStep == numberPressed-1) {
//			RenderablePoint point = new RenderablePoint((int)x, (int)y);
//			point.setProperties(Color.RED, 3.0f);
//			gui.draw(point);
//			}
			
			a = a - Math.PI/6;
		}
	}
	
	public void checkXYParameters() {
		
//		HOW TO READ Input???
		
//		if (textFieldX > 0 && textFieldX > 0 && textFieldY < 480 && textFieldY < 480) {
//			System.out.println("Good paramteters!");
//			
//			System.out.println("textFieldX: " + textFieldX);
//			System.out.println("textFieldY: " + textFieldY);
//		}
//		else {
//			System.out.println("Bad Parameters! Turn to default values!");
//			textFieldX = 50;
//			textFieldY = 50;
//		}
//		textFieldX 
//		textFieldY
		
		boolean  notFoundGoodPoints = true;
		
		while(notFoundGoodPoints) {
			Random generator = new Random();
			textFieldX = generator.nextInt(800);
			textFieldY = generator.nextInt(800);
			if (!checkIfInObstacle (new IntPoint(textFieldX, textFieldY))) {
				notFoundGoodPoints = false;
			}
		}
		
//		textFieldX = 50;
//		textFieldY = 50;
	}
	
	public boolean checkIfInObstacle(IntPoint pointToCheck) {

		int i;
		int j;
		boolean result = false;
		for (Obstacle obstacle : obstacles) {
			for (i = 0, j = obstacle.vertex.length - 1; i < obstacle.vertex.length; j = i++) {
				if ((obstacle.vertex[i].y > pointToCheck.y) != (obstacle.vertex[j].y > pointToCheck.y)
						&& (pointToCheck.x < (obstacle.vertex[j].x - obstacle.vertex[i].x)
								* (pointToCheck.y - obstacle.vertex[i].y)
								/ (obstacle.vertex[j].y - obstacle.vertex[i].y)
								+ obstacle.vertex[i].x)) {
					result = !result;
				}
			}
			if (result) {
				return result;
			}
		}
		System.out.println("result: " + result);
		return result;
	}
	
	// calculating gold potential and put fill goldPotential array
	public void calculatingGoldPotential() {
		for(int i = 0; i < robot.rayPoints.length; i++)		
			robot.rayPoints[i].setGoldPotential(
					1000.0/calculateDistanceBetweenPoints(goldPoint.x, goldPoint.y, 
							robot.rayPoints[i].getPoint().x, robot.rayPoints[i].getPoint().y));
		
	}
	

	
	public boolean checkIfReachGoldCircle() {
		if ((int) (Math.pow(robot.getCoordinates().x - goldPoint.x, 2) + Math.pow(
				robot.getCoordinates().y - goldPoint.y, 2)) <= Math.pow(goalRadius, 2)) {
			System.out.println("first expression: " + Math.pow(robot.getCoordinates().x - goldPoint.x, 2));
			System.out.println("first expression: " + Math.pow(robot.getCoordinates().y - goldPoint.y, 2));
			return true;
		} else {
			return false;
		}
	}
	
	public RenderablePolygon makePolygonObstacles(int xCoordinate, int yCoordinate, int width, int breadth) {
		
		RenderablePolygon rendPolygon = new RenderablePolygon();
		rendPolygon.addVertex(xCoordinate, yCoordinate);
		
		RenderableString stringForPath1 = new RenderableString (xCoordinate, yCoordinate, 
				"(" + (xCoordinate) + "," + (yCoordinate) + ")");
		stringForPath1.setProperties(Color.BLACK, font);
		gui.draw(stringForPath1);
		
		rendPolygon.addVertex(xCoordinate + width, yCoordinate);
		RenderableString stringForPath2 = new RenderableString (xCoordinate + width, yCoordinate, 
				"(" + (xCoordinate + width) + "," + (yCoordinate) + ")");
		stringForPath2.setProperties(Color.BLACK, font);
		gui.draw(stringForPath2);
		
		rendPolygon.addVertex(xCoordinate + width, yCoordinate + breadth);
		RenderableString stringForPath3 = new RenderableString (xCoordinate + width, yCoordinate + breadth, 
				"(" + (xCoordinate + width) + "," + (yCoordinate + breadth) + ")");
		stringForPath3.setProperties(Color.BLACK, font);
		gui.draw(stringForPath3);
		
		rendPolygon.addVertex(xCoordinate, yCoordinate + breadth);
		RenderableString stringForPath4 = new RenderableString (xCoordinate, yCoordinate + breadth, 
				"(" + (xCoordinate) + "," + (yCoordinate + breadth) + ")");
		stringForPath4.setProperties(Color.BLACK, font);
		gui.draw(stringForPath4);
		
		rendPolygon.setProperties(Color.GREEN, false);
		gui.draw(rendPolygon);
		return rendPolygon;
	}
	
	// returns the number of element in array with max gold potential
	public int getMaxGoldPotential() {
		double minDistance = 10000000;
		int numberOfElement = -1;
		for(int i = 0; i < robot.rayPoints.length; i++) {
			if (robot.rayPoints[i].getGoldPotential() < minDistance) {
				minDistance = robot.rayPoints[i].getGoldPotential();
				numberOfElement = i;
			}
		}
		if (numberOfElement != 3) {
			numberOfTurns ++;
		}	
		robot.setAngle(robot.rayPoints[numberOfElement].getAngle());
		return numberOfElement;
	}
}